/*
 * kdtree_impl.h
 *
 *  Created on: May 1, 2011
 *      Author: tqlong
 */

#ifndef KDTREE_IMPL_H_
#define KDTREE_IMPL_H_
#include <QtCore/QVector>
#include <cassert>

template<typename vec_type>
BoundingBox<vec_type>::BoundingBox(int dim) :
  m_min(dim), m_max(dim), n_dim(dim)
{
  m_min.fill(POSITIVE_INFINITY);
  m_max.fill(NEGATIVE_INFINITY);
}

template<typename vec_type>
void BoundingBox<vec_type>::accumulate(const vec_type& v)
{
  assert(v.n_elem == n_dim);
  for (int i = 0; i < n_dim; i++)
    {
      if (m_min[i] > v[i])
        m_min[i] = v[i];
      if (m_max[i] < v[i])
        m_max[i] = v[i];
    }
}

template<typename vec_type>
void BoundingBox<vec_type>::accumulate(const BoundingBox& b)
{
  assert(b.n_dim == n_dim);
  for (int i = 0; i < n_dim; i++)
    {
      if (m_min[i] > b.m_min[i])
        m_min[i] = b.m_min[i];
      if (m_max[i] < b.m_max[i])
        m_max[i] = b.m_max[i];
    }
}

template<typename vec_type>
double BoundingBox<vec_type>::lowerBound(const vec_type& v, double norm) const
{
  double s = 0;
  vec_type proj(v);
  for (int i = 0; i < n_dim; i++)
    {
      proj[i] = 0;
      if (v[i] < m_min[i]) proj[i] = m_min[i] - v[i];
//        s += (v[i] - m_min[i]) * (v[i] - m_min[i]);
      if (v[i] > m_max[i]) proj[i] = v[i] - m_max[i];
//        s += (v[i] - m_max[i]) * (v[i] - m_max[i]);
    }
  return norm == std::numeric_limits<double>::infinity() ? arma::norm(proj, "inf") : arma::norm(proj, (int)norm);
//  return s;
}

template<typename vec_type>
void BoundingBox<vec_type>::print() const
{
  std::cerr << "min=\n" << m_min << "max=\n" << m_max << std::endl;
}

template<typename DATA>
void KDTree<DATA>::buildTree()
{
  split();
  for_all_node_child(i)
    child(i)->buildTree();
  if (this->n_child() == 0) {
    for_all_node_data(i) m_box.accumulate(this->get(i));
  }
  else {
    for_all_node_child(i) m_box.accumulate(child(i)->m_box);
  }
}

template<typename DATA>
void KDTree<DATA>::split()
{
  //    Log::m_log << "split ms=" << m_s << " me=" << m_e << endl;
  //    for_all_node_data(i)
  //      Log::m_log << m_idx[m_s+i] << " ";
  //    Log::m_log << endl;
  if (this->n() <= 10)
    return;
  double variance = 0, split_value = 0;
  int dim = 0;
  variance = find_max_variance(dim);
  //    Log::m_log << "dim=" << dim << " var=" << variance << endl;
  if (variance < 1e-10 || dim < 0)
    return;
  split_value = find_split_value(dim);
  //    Log::m_log << "val=" << split_value << endl;
  split(dim, split_value);
}

template<typename DATA>
double KDTree<DATA>::find_max_variance(int& dim_max) const
{
  double max_variance = 0;
  dim_max = -1;
  for (int dim = 0; dim < this->n_dim(); dim++)
    {
      double min = min_dim(dim), max = max_dim(dim), variance = max - min;
      if (max_variance < variance)
        {
          dim_max = dim;
          max_variance = variance;
        }
    }
  return max_variance;
}

template<typename DATA>
double KDTree<DATA>::find_split_value(int dim) const
{
  QVector<double> x(this->n());
  for_all_node_data(i)
    x[i] = this->get(i, dim);
  qSort(x.begin(), x.end());

  int nn = this->n();
  double split_value = x[nn / 2];
  double max_gap = 0.0;
  if (nn / 2 - 1 >= 0)
    {
      split_value = (x[nn / 2] + x[nn / 2 - 1]) / 2;
      max_gap = x[nn / 2] - x[nn / 2 - 1];
    }

  for (int i = nn / 4; i < 3 * nn / 4 - 1; i++)
    {
      double gap = x[i + 1] - x[i];
      if (gap > max_gap)
        {
          max_gap = gap;
          split_value = (x[i + 1] + x[i]) / 2;
        }
    }
  return split_value;
}

template<typename DATA>
double KDTree<DATA>::min_dim(int dim) const
{
  double min = std::numeric_limits<double>::infinity();
  for_all_node_data(i)
    {
      double v = this->get(i, dim);
      if (v < min)
        min = v;
    }
  return min;
}

template<typename DATA>
double KDTree<DATA>::max_dim(int dim) const
{
  double max = -std::numeric_limits<double>::infinity();
  for_all_node_data(i)
    {
      double v = this->get(i, dim);
      if (v > max)
        max = v;
    }
  return max;
}

template<typename DATA>
void KDTree<DATA>::split(int dim, double split_value)
{
  int n_left = 0;
  int nn = this->n();

  for_all_node_data(i)
    {
      double v = this->get(i, dim);
//      Log::m_log << "v=" << v << " ";
      if (v <= split_value)
        {
//          Log::m_log << this->origIdx(i) << " --> [" << this->m_s+n_left << "]";
          this->swap(n_left, i);
//          Log::m_log << "="<<this->origIdx(n_left) << " ";
          n_left++;
        }
    }
  //    Log::m_log << endl;
  //    Log::m_log << "n_left="<< n_left << endl;
  if (n_left > 0 && n_left < nn)
    {
      //      Log::m_log << "create left child ms="<< m_s << " me=" << n_left+m_s << endl;
      //      Log::m_log << "create right child ms="<< m_s+n_left << " me=" << n()+m_s << endl;
      new KDTree(this, 0, n_left);
      new KDTree(this, n_left, nn);
    }
}

template<typename DATA>
void KDTree<DATA>::buildStats()
{
//  Log::m_log << "dim="<< m_box.dim() << endl;
  for (int i = 0; i < this->n_child(); i++)
    child(i)->buildStats();

  if (this->n_child() == 0)
    for_all_node_data(i) m_box.accumulate(this->get(i));
  else
    for_all_node_child(i) m_box.accumulate(child(i)->m_box);
}

template<typename DATA>
void KDTree<DATA>::load(const std::string& fName)
{
  __Base::load(fName);
  buildStats();
}

#endif /* KDTREE_IMPL_H_ */
